import numpy
from numpy import linalg as LA
from obstacle import *
from math import sqrt
import helper

class Plane(Obstacle):
    """The class representing a Plane."""
	
    state           = 0;
    speed           = 0;
    resizeSpeed     = 0;
    futureDiagVecX  = 0;
    rotSpeed        = 0;
    collisionCheat  = 0.008;
	
    tempVector      = numpy.zeros((3),   dtype=float);
    diagVec         = numpy.zeros((3),   dtype=float);
    dir             = numpy.zeros((3),   dtype=float);
    middle          = numpy.zeros((3),   dtype=float);
    normal          = numpy.zeros((3),   dtype=float);
    vertices        = numpy.zeros((4,3), dtype=float);
    boundaryNormals = numpy.zeros((4,3), dtype=float);
    normals	        = numpy.zeros((4,3), dtype=float);
    indices	        = numpy.zeros((4),   dtype=int);
    inverseNormals  = numpy.zeros((4,3), dtype=float);
    rotAxis	        = numpy.zeros((3),   dtype=float);

    def __init__(self, mX, mY, mZ, nX, nY, nZ, size):
        self.middle = numpy.array([mX, mY, mZ]);
	    
#        print self.middle
#        print size
        if all(self.middle == 0):
            self.middle[0] += 0.01;
		
        len = sqrt(nX*nX + nY*nY + nZ*nZ);
		
        if len != 0:
            self.normal = numpy.array([nX/len , nY/len, nZ/len]);
        else:
            self.normal = numpy.array([0, 1, 0]);
			
        vecY = numpy.array([0, 1, 0]);
        vecX = numpy.array([1, 0, 0]);	
		
        temp = numpy.cross(self.normal, vecY);
		
#        print 'temp';
#        print temp;
     
        if all(temp == 0):
		    temp = numpy.cross(self.normal, vecX);

#        print temp;

        vectNorm = LA.norm(temp);
        temp = temp / vectNorm;
        self.vertices[0] = temp * size;
        
#        print 'vertices 0'
#        print self.vertices[0];
#        print self.middle;
        
        temp = self.vertices[0] - self.middle;
		
        temp = numpy.cross(self.normal, temp);
        vectNorm = LA.norm(temp);
        temp = temp / vectNorm;
        self.vertices[1] = temp * size;
        temp = self.vertices[1] - self.middle;
		
        temp = numpy.cross(self.normal, temp);
        vectNorm = LA.norm(temp);
        temp = temp / vectNorm;
        self.vertices[2] = temp * size;
        temp = self.vertices[2] - self.middle;
		
        temp = numpy.cross(self.normal, temp);
        vectNorm = LA.norm(temp);
        temp = temp / vectNorm;
        self.vertices[3] = temp * size;
        
        self.vertices[0] = numpy.array([self.middle[0] - size /2 , self.middle[1], self.middle[2] + size /2]);
        self.vertices[1] = numpy.array([self.middle[0] + size /2 , self.middle[1], self.middle[2] + size /2]);
        self.vertices[2] = numpy.array([self.middle[0] + size /2 , self.middle[1], self.middle[2] - size /2]);
        self.vertices[3] = numpy.array([self.middle[0] - size /2 , self.middle[1], self.middle[2] - size /2]);
        
#        print 'vertices 2'
#        print self.vertices;
        
        self.diagVec = numpy.array([abs(self.vertices[0][0] - self.middle[0]),
		abs(self.vertices[0][1] - self.middle[1]),
		abs(self.vertices[0][2] - self.middle[2])]);
		
        self.futureDiagVecX = self.diagVec[0];
        self.resizeSpeed = 1;
        self.normals = numpy.array([self.normal, self.normal, self.normal, self.normal]);
        self.inverseNormals = -self.normals;
        self.colorFront = numpy.array([1, 1, 0, 1]);
        self.colorBack = numpy.array([0, 1, 1, 1]);
        self.indices = numpy.array([0, 1, 2, 3]);
        self.BoundNormals(); 	
        
    def StaticMove(self, dx, dy, dz):
        tempArray = numpy.array([dx, dy, dz]);
        self.middle += tempArray;	
        
        for i in range(0,4):
            numpy.add(self.vertices[i], tempArray, self.vertices[i]);
            	
        self.BoundNormals(); 
    def StaticResize(self, factor):
	    for i in self.vertices:
			tempArray = i - self.middle;
			i = self.middle + factor * tempArray;
	    self.diagVec = abs(self.vertices[0] - self.middle);
	    self.futureDiagVecX = self.diagVec[0];
	    self.resizeSpeed = 1; # be careful...
	    self.BoundNormals(); 
    def Resize(self, factor, speed):
		if speed != 0:
		    self.resizeSpeed = speed;
		    self.futureDiagVecX = factor * self.diagVec[0];
		else:
			self.resizeSpeed = 1;
			self.futureDiagVecX = self.diagVec[0];
    def Move(self, x, y, z, speed):
        len = sqrt(x*x + y*y + z*z);
	    
        if len != 0:
            self.dir = numpy.array(x / len, y / len, z / len);
        else:
            self.dir = numpy.zeros(3);
			
        self.speed = speed;
    def SetState(self, newState):
        self.state = newState;
    def Rotate(self, x, y, z, speed):
        self.rotAxis = numpy.array([x, y, z]);
        self.rotSpeed = speed;
        R = helper.rotate_matrix(self.rotAxis, self.rotSpeed)
        T = numpy.matrix(numpy.empty_like(self.vertices))
        T[:] = self.middle
        vertices = numpy.matrix(self.vertices)
        vertices[:] -= T
        vertices[:] *= numpy.transpose(R)
        vertices[:] += T
        self.vertices[:] = vertices[:]
        
        normal = numpy.matrix(self.normal)
        normal = normal * numpy.transpose(R)
        self.normal[:] = normal[:]
                
        self.normals = numpy.array([self.normal, self.normal, self.normal, self.normal]);
        self.inverseNormals = -self.normals;
        self.BoundNormals()
        
    def BoundNormals(self):
        self.boundaryNormals[0] = self.vertices[1] - self.vertices[0];
        self.boundaryNormals[1] = self.vertices[2] - self.vertices[1];
        self.boundaryNormals[2] = self.vertices[3] - self.vertices[2];
        self.boundaryNormals[3] = self.vertices[0] - self.vertices[3];
        self.boundaryNormals[0] /= LA.norm(self.boundaryNormals[0]);
        self.boundaryNormals[1] /= LA.norm(self.boundaryNormals[1]);
        self.boundaryNormals[2] /= LA.norm(self.boundaryNormals[2]);
        self.boundaryNormals[3] /= LA.norm(self.boundaryNormals[3]);
        
    def CollisionHandle(self, point):
        threshold = 0.15;
        
        t = numpy.dot(self.boundaryNormals[0], point - self.vertices[0]);
        if t < threshold:
            return point;
        t = numpy.dot(self.boundaryNormals[1], point - self.vertices[1]);
        if t < threshold:
            return point;
        t = numpy.dot(self.boundaryNormals[2], point - self.vertices[2]);
        if t < threshold:
            return point;
        t = numpy.dot(self.boundaryNormals[3], point - self.vertices[3]);
        if t < threshold:
            return point; 
        
#         plane collision
        
        x = numpy.dot(self.normal, point - self.middle);
        
        if x <= threshold: 
            point = point - x * self.normal + self.collisionCheat * self.normal;
            return point;
        
        return point;
        
    def StaticRotate(self, x, y, z, angle):
#        Will not be implemented in the first release
        raise NotImplementedError( "Plane implementation should come here..." )